/*
 * heightfield.h
 *
 *  Created on: 17.09.2013
 *      Author: alex
 */
#ifndef HEIGHTFIELD_H_
#define HEIGHTFIELD_H_

#include <boost/shared_ptr.hpp>
#include <bullet/LinearMath/btVector3.h>
#include <ros/publisher.h>
#include <std_msgs/ColorRGBA.h>
#include <bullet_rviz_test/physical_object.h>

// Bullet type forward declarations:
class btMotionState;
class btRigidBody;
class btHeightfieldTerrainShape;

namespace bullet_rviz_test
{

class Heightfield: public PhysicalObject
{
public:
  Heightfield(const btVector3 & extents, const float * data, size_t size_x,
      size_t size_y);
  ~Heightfield();

  btVector3 getExtents();
  const float * getData();
  float getData(size_t x_index, size_t y_index);
  float getHeightAtPosition(btScalar x, btScalar y);
  size_t getSizeX();
  size_t getSizeY();
  btMotionState * getMotionState();

  virtual btRigidBody * getRigidBody();

private:
  Heightfield(const Heightfield &);
  Heightfield & operator=(const Heightfield &);

  btVector3 extents_;
  float * data_;
  size_t size_x_;
  size_t size_y_;

  btMotionState * motion_state_;
  btHeightfieldTerrainShape * heightfield_terrain_shape_;
  btRigidBody * rigid_body_;
};

typedef boost::shared_ptr<Heightfield> HeightfieldPtr;

}
#endif /* HEIGHTFIELD_H_ */
